/***************************************************************************
 创建者: 彭秋明
 开始时间: 201７.02.24
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *　对机器人模型进行设计，添加轴和设置关节参数
 *　其中Puma560类型设计成６轴，采用standard ＤＨ参数法
 *　KukaLWR_DHnew类型设计成８轴，采用modified DH参数法
 ***************************************************************************/

#include "models.hpp"

namespace KDL
{
    Chain Puma560()
    {
        Chain puma560;

        puma560.addSegment(Segment());

        //joint 0
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0,M_PI_2,0.0,0.0),
                                   RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0))));

        //joint 1
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.4318,0.0,0.0,0.0),
                                   RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0))));

        puma560.addSegment(Segment());

        //joint 2
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0203,-M_PI_2,0.15005,0.0),
                                   RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0))));

        //joint3
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0,M_PI_2,0.4318,0.0),
                                   RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0))));

        puma560.addSegment(Segment());

        puma560.addSegment(Segment());

        //joint 4
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0,-M_PI_2,0.0,0.0),
                                   RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0))));

        //joint 5
        puma560.addSegment(Segment(Joint(Joint::RotZ),
                                   Frame::DH(0.0,0.0,0.0,0.0),
                                   RigidBodyInertia(0.09,Vector(0,0,.032),RotationalInertia(.15e-3,0.15e-3,.04e-3,0,0,0))));

        puma560.addSegment(Segment());

        return puma560;

    }

    Chain KukaLWR_DHnew()
    {
        Chain kukaLWR_DHnew;

        //joint 0
        kukaLWR_DHnew.addSegment(Segment(Joint(Joint::None),
                  Frame::DH_Craig1989(0.0, 0.0, 0.31, 0.0)));

        //joint 1
        kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
                  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
                  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
                                                 Vector::Zero(),
                                                 RotationalInertia(0.0,0.0,0.0115343,0.0,0.0,0.0))));

        //joint 2
        kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
                  Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0),
                  Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0).Inverse()*RigidBodyInertia(2,
                                                   Vector(0.0,-0.3120511,-0.0038871),
                                                   RotationalInertia(-0.5471572,-0.0000302,-0.5423253,0.0,0.0,0.0018828))));

        //joint 3
        kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
                  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
                  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
                                                   Vector(0.0,-0.0015515,0.0),
                                                   RotationalInertia(0.0063507,0.0,0.0107804,0.0,0.0,-0.0005147))));

        //joint 4
        kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
                  Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0),
                  Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0).Inverse()*RigidBodyInertia(2,
                                                   Vector(0.0,0.5216809,0.0),
                                                   RotationalInertia(-1.0436952,0.0,-1.0392780,0.0,0.0,0.0005324))));

        //joint 5
        kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
                  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
                  Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
                                                   Vector(0.0,0.0119891,0.0),
                                                   RotationalInertia(0.0036654,0.0,0.0060429,0.0,0.0,0.0004226))));

        //joint 6
        kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
                  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
                  Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
                                                   Vector(0.0,0.0080787,0.0),
                                                   RotationalInertia(0.0010431,0.0,0.0036376,0.0,0.0,0.0000101))));
        //joint 7
        kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
                   Frame::Identity(),
                   RigidBodyInertia(2,
                                                   Vector::Zero(),
                                                   RotationalInertia(0.000001,0.0,0.0001203,0.0,0.0,0.0))));

        return kukaLWR_DHnew;

    }

}
